package com.testcolision.simulation;

import com.badlogic.gdx.Gdx;
import com.badlogic.gdx.math.MathUtils;
import com.badlogic.gdx.math.Vector2;

public class RigidBody {
	// linear properties
	public Vector2 m_position = new Vector2();
	public Vector2 m_velocity = new Vector2();
	public Vector2 m_forces = new Vector2();
	private float m_mass;
	public float m_drag;

	// angular properties
	public float m_angle;
	private float m_angularVelocity;
	private float m_torque;
	private float m_inertia;

	private Vector2 m_halfSize = new Vector2();

	public RigidBody() {
		// set these defaults so we dont get divide by zeros
		m_mass = 1.0f;
		m_inertia = 1.0f;
	}

	// intialize out parameters
	public void Setup(Vector2 halfSize, float mass) {
		// store physical parameters
		m_halfSize = halfSize;
		m_mass = mass;

		m_inertia = (1.0f / 12.0f) * (halfSize.x * halfSize.x) * (halfSize.y * halfSize.y) * mass;
	}

	public void SetLocation(Vector2 position, float angle) {
		m_position.set(position);
		m_angle = angle;
	}

	public Vector2 GetPosition() {
		return m_position.cpy();
	}

	public void Update(float timeStep) {
		// integrate physics
		// linear
		Vector2 acceleration = m_forces.cpy().mul(1 / m_mass);
		m_velocity.add(acceleration.mul(timeStep));
		m_position.add(m_velocity.cpy().mul(timeStep));
		m_forces.set(0, 0); // clear forces

		// angular
		float angAcc = m_torque / m_inertia;
		m_angularVelocity += angAcc * timeStep;
		m_angle += m_angularVelocity * timeStep;
		m_torque = 0; // clear torque
	}

	// take a relative vector and make it a world vector
	public Vector2 RelativeToWorld(Vector2 relative) {

		return relative.cpy().rotate(m_angle * MathUtils.radiansToDegrees);
	}

	// take a world vector and make it a relative vector
	public Vector2 WorldToRelative(Vector2 world) {
		return world.cpy().rotate(-m_angle * MathUtils.radiansToDegrees);
	}

	// velocity of a point on body
	public Vector2 PointVel(Vector2 worldOffset) {
		Vector2 tangent = new Vector2().set(-worldOffset.y, worldOffset.x);
		return tangent.mul(m_angularVelocity).add(m_velocity);
	}

	public void AddForce(Vector2 worldForce, Vector2 worldOffset) {
		// add linar force
		m_forces.add(worldForce);

		// and it's associated torque
		m_torque += worldOffset.crs(worldForce);
	}

}
